#include "HALL_Function.h"
#include "Parameter.h"
#include "SVPWM.h"
#include "FOC.h"
#include "DAC.h"

void Theta_Calculate(void)
{
	  Sin_Normalization=((Sin_value-sin_dc_component)*10000)/(sin_max-sin_dc_component);
	  Cos_Normalization=((Cos_value-cos_dc_component)*10000)/(cos_max-cos_dc_component);
		Angle_PID();
}


void Speed_Calculate(void)
{
	if(speed_count>7)
		{
			speed_count=0;
			speed_get_flag=1;
		}
		
		if(speed_get_flag==0)
		{
			speed_time[speed_count]=((CCU40_CC42->CV[0]&0x0000FFFF)<<16)+(CCU40_CC41->CV[0]&0x0000FFFF);
		  speed_time_sum += speed_time[speed_count];
		}
		else
		{
			speed_time_sum -=speed_time[speed_count];
			speed_time[speed_count] = ((CCU40_CC42->CV[0]&0x0000FFFF)<<16)+(CCU40_CC41->CV[0]&0x0000FFFF);
			speed_time_sum += speed_time[speed_count];
			Speed=(112500000*8)/speed_time_sum;	
		}
	
		speed_count++;	
} 

void Angle_Offset(void)
{
	if(Speed>500)
	{
   Angle_offset = ((4*Speed)/100)+30;
	if(Angle_offset>200)
	{
		Angle_offset = 200;
	}
  }
}
